#ifndef _KRIGING_H
#define _KRIGING_H

#include <cmath>
#include <vector>
extern "C"{
#include <matrix2.h>	// header for mesch12b
}
#include "../al_structs.h"


template<typename ITER>
class TKriging
{
private:
	MAT* m_matA;
	PERM	*pivot;
	//vector<int> m_Permutation;
	size_t m_nSize;
	double Xmin,Xmax;
	double Ymin,Ymax;
	
	ITER iterF;
	double m_dSemivariance;
	
	const double GetXYDist(alPoint3D &p1,alPoint3D &p2){
		return sqrt( (p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y) );
	}
	
public:
	TKriging(const ITER first, const ITER last, double dSemivariance) : m_dSemivariance(dSemivariance), m_matA(0),pivot(0),iterF(first){
		m_nSize = 0;
		ITER start = first;
		while(start != last) {
			Xmax=AL_GET_MAX((*start).x  , Xmax);
			Ymax=AL_GET_MAX((*start).y  , Ymax);
			Xmin=AL_GET_MIN((*start).x  , Xmin);
			Ymin=AL_GET_MIN((*start).y  , Ymin);
			++m_nSize;
			++start;
		}
		
		m_matA = m_get(m_nSize,m_nSize);
		for(size_t j=0; j<m_nSize; j++) {
			for(size_t i=0; i<m_nSize; i++) {
				if(i == m_nSize-1 || j == m_nSize-1) {
					m_matA->me[i][j] = 1;
					if(i == m_nSize-1 && j == m_nSize-1)
						m_matA->me[i][j] = 0;
					continue;
				}
				m_matA->me[i][j] = GetXYDist(*(first+i), *(first+j)) * dSemivariance;
			}
		}

		pivot = px_get(m_matA->m);
		LUfactor(m_matA,pivot);
		/* set values of b here */
		//x = LUsolve(LU,pivot,b,VNULL);
	}
	
	~TKriging(){
		M_FREE(m_matA);
		PX_FREE(pivot);
	}
	
	double GetInterpolatedZ(double xpos, double ypos){
		VEC *b = v_get( m_nSize ), *x;
		for(size_t i=0;i<m_nSize-1;i++)
			b->ve[i] = GetXYDist(alPoint3D(xpos,ypos,0.0), *(iterF+i)) * m_dSemivariance;
		b->ve[m_nSize-1] = 1.0;
		
		x = LUsolve(m_matA,pivot,b,VNULL);
		double z=0.0;
		
		for(size_t i=0;i<m_nSize-1;i++)
			z+= x->ve[i] * (*(iterF+i)).z;
		
		V_FREE(x);
		V_FREE(b);
		return z;
	}
	
	void GetXRange(double &min, double &max){min=Xmin;max=Xmax;}
	void GetYRange(double &min, double &max){min=Ymin;max=Ymax;}
	
};

typedef TKriging< std::vector<alPoint3D>::iterator > KrigingV;
typedef TKriging< alPoint3D * > KrigingA;

#endif